﻿using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;

/// <summary>
/// A*寻路算法
/// </summary>
public static class AStarPathFinding
{
    /// <summary>
    /// 寻路中的点
    /// </summary>
    private class AStarPoint
    {
        public Vector2Int curPos;
        public float curCost;
        public float predictCost;
        public AStarPoint lastPoint;
        public AStarPoint(Vector2Int curPos, float curCost, float predictCost, AStarPoint lastPoint)
        {
            this.curPos = curPos;
            this.curCost = curCost;
            this.predictCost = predictCost;
            this.lastPoint = lastPoint;
        }
    }
    /// <summary>
    /// 寻路
    /// </summary>
    /// <param name="validPos">可经过的点</param>
    /// <param name="originalPos">起始点</param>
    /// <param name="targetPos">目标点</param>
    /// <returns>路径点</returns>
    public static List<Vector2Int> PathFinding(HashSet<Vector2Int> validPos, Vector2Int originalPos, Vector2Int targetPos)
    {
        HashSet<AStarPoint> posSet = new HashSet<AStarPoint>();
        HashSet<Vector2Int> posFlagSet = new HashSet<Vector2Int>();
        AStarPoint aStarPoint = new AStarPoint(originalPos, 0, Util.GetManhattanDistance(originalPos, targetPos), null);
        posSet.Add(aStarPoint);
        posFlagSet.Add(originalPos);
        while (posSet.Count > 0)
        {
            AStarPoint curAStarPoint = null;
            float minDistance = posSet.Min(x => x.curCost + x.predictCost);
            foreach (AStarPoint point in posSet)
            {
                if (point.curCost + point.predictCost == minDistance)
                {
                    curAStarPoint = point;
                    break;
                }
            }
            posSet.Remove(curAStarPoint);
            foreach (var dir in Direction2D.cardinalDirectionList)
            {
                Vector2Int newPos = curAStarPoint.curPos + dir;
                if (validPos.Contains(newPos) && !posFlagSet.Contains(newPos))
                {
                    aStarPoint = new AStarPoint(newPos, curAStarPoint.curCost + 1, Util.GetManhattanDistance(newPos, targetPos), curAStarPoint);
                    posSet.Add(aStarPoint);
                    posFlagSet.Add(aStarPoint.curPos);
                    if (newPos == targetPos)
                    {
                        posSet.Clear();
                        break;
                    }
                }
            }
        }
        List<Vector2Int> pathList = new List<Vector2Int>();
        Stack<Vector2Int> pathStack = new Stack<Vector2Int>();
        if(aStarPoint.curPos != targetPos)
        {
            return pathList;
        }
        while (aStarPoint != null)
        {
            pathStack.Push(aStarPoint.curPos);
            aStarPoint = aStarPoint.lastPoint;
        }
        while (pathStack.Count > 0)
        {
            pathList.Add(pathStack.Pop());
        }
        return pathList;
    }
}
